/*写一个pid控制器*/

class pid
{
private:
    /* data */
    float p;
    float i;
    float d;
    float i_max; // 积分最大
    float max;   // 数据最大
    float i_out; //
    float out;
    float d_out;
    float l_val;
    float N = 5; // 20Hz
    bool is_first = true;
    float p_out = 0;
    // bool is_first;   //第一次赋值
public:
    pid(/* args */)
    {
        p = 0;
        i = 0;
        d = 0;
        i_max = 0; // 积分最大
        max = 0;   // 数据最大
        i_out = 0; //
        out = 0;
        l_val = 0;
        // is_first = true;
    }
    pid(float p, float i, float d, float i_max, float max)
    {
        this->p = p;
        this->i = i;
        this->d = d;
        this->i_max = i_max;
        this->max = max;
        i_out = 0;
        out = 0;
        l_val = 0;
        // is_first = true;
    }

    ~pid() {}

    float get_p()
    {
        return p;
    }
    void set_p(float value)
    {
        p = value;
    }

    float get_i()
    {
        return i;
    }
    void set_i(float value)
    {
        i = value;
    }

    float get_d()
    {
        return d;
    }
    void set_d(float value)
    {
        d = value;
    }

    float get_i_max()
    {
        return i_max;
    }
    void set_i_max(float value)
    {
        i_max = value;
    }
    float get_max()
    {
        return max;
    }
    void set_max(float value)
    {
        max = value;
    }
    float get_p_out()
    {
        return p_out;
    }
    float get_i_out()
    {
        return i_out;
    }
    void set_i_out(float value)
    {
        i_out = value;
    }

    float get_out()
    {
        return out;
    }

    void set_out(float value)
    {
        i_out = value - p_out;
    }

    float calc_pid(float val)
    {
        p_out = val * p;
        i_out += val * i;
        if (i_out > i_max)
        {
            i_out = i_max;
        }
        else if (i_out < -i_max)
        {
            i_out = -i_max;
        }
        /* 计算D */
        if (is_first)
        {
            d_out = 0;
            l_val = val;
            is_first = false;
        }
        else
        {
            d_out = d * N * (val - l_val) + (1 - N * 0.01f) * d_out;
            l_val = val;
        }

        float ou = p_out + i_out + d_out;
        if (ou > max)
        {
            out = max;
            i_out -= (ou - max) / 3; // 防止过饱和
        }
        else if (ou < -max)
        {
            out = -max;
            i_out -= (ou + max) / 3; // 防止过饱和
        }
        else
        {
            out = ou;
        }
        return out;
    }

    float calc_pi(float val)
    {
        p_out = val * p;
        i_out += val * i;
        if (i_out > i_max)
        {
            i_out = i_max;
        }
        else if (i_out < -i_max)
        {
            i_out = -i_max;
        }
        float ou = p_out + i_out;
        if (ou > max)
        {
            out = max;
            // i_out + p_out - max
            i_out -= (ou - max) / 3; // 防止过饱和
        }
        else if (ou < -max)
        {
            out = -max;
            i_out -= (ou + max) / 3; // 防止过饱和
        }
        else
        {
            out = ou;
        }
        return out;
    }

    void clear()
    {
        i_out = 0;
        out = 0;
        d_out = 0;
        l_val = 0;
        is_first = true;
    }
};
